package org.pageley.games.domain.environment.pathfinding;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;

import org.pageley.games.domain.Direction;
import org.pageley.games.domain.Location;
import org.pageley.games.domain.environment.Node;

public class AStarAlgorithm implements PathFindingAlgorithm {

	private DistanceHeuristic heuristic;
	private boolean allowDiagonal;
	
	public AStarAlgorithm(DistanceHeuristic heuristic, boolean allowDiagonal) {
		this.heuristic = heuristic;
		this.allowDiagonal = allowDiagonal;
	}
	
	@Override
	public List<Node> findPath(Node start, Node end,
			Map<Location, Node> nodes) {
		List<Node> result = new ArrayList<Node>();
		if(nodes.containsValue(end)) {
			Collection<Node> evaluatedNodes = new HashSet<Node>();
			Collection<Node> tentativeNodes = new HashSet<Node>();
			for (Node node : nodes.values()) {
				tentativeNodes.add(node);
			}
			Map<Node, Double> distanceFromStart = new HashMap<Node, Double>();
			Map<Node, Double> heuristicDistanceToGoal = new HashMap<Node, Double>();
			Map<Node, Double> estimatedTotalDistanceToGoal = new HashMap<Node, Double>();
			Map<Node, Node> trail = new HashMap<Node, Node>();
	
			distanceFromStart.put(start, 0.0);
			heuristicDistanceToGoal.put(start, heuristic.estimateDistance(start, end));
			estimatedTotalDistanceToGoal.put(start, heuristicDistanceToGoal
					.get(start));
			while (!tentativeNodes.isEmpty()) {
				Node nodeToTry = null;
				Double minDistance = Double.MAX_VALUE;
				for (Node n : tentativeNodes) {
					Double d = estimatedTotalDistanceToGoal.get(n);
					if (d != null && d < minDistance) {
						nodeToTry = n;
						minDistance = d;
					}
				}
				if (nodeToTry == null) {
					break;
				}
				if (nodeToTry.equals(end)) {
					result = reconstructPath(trail, end);
				}
				tentativeNodes.remove(nodeToTry);
				evaluatedNodes.add(nodeToTry);
				for (Node n : getNeighbors(nodeToTry, nodes)) {
					if (!evaluatedNodes.contains(n)) {
						Node node = n;
						Double tentativeDistanceFromStart = distanceFromStart.get(nodeToTry)
								+ nodeToTry.distanceTo(node);
						boolean tentativeBetter = false;
						if (!tentativeNodes.contains(n)) {
							tentativeNodes.add(n);
							heuristicDistanceToGoal.put(node, heuristic.estimateDistance(node, end));
							tentativeBetter = true;
						} else if (distanceFromStart.get(node) == null
								|| tentativeDistanceFromStart < distanceFromStart.get(node)) {
							tentativeBetter = true;
						}
						if (tentativeBetter) {
							trail.put(node, nodeToTry);
							distanceFromStart.put(node,
									tentativeDistanceFromStart);
							Double heuristicDistance = heuristicDistanceToGoal
									.get(node);
							if (heuristicDistance == null) {
								heuristicDistance = 0.0;
							}
							estimatedTotalDistanceToGoal.put(node,
									distanceFromStart.get(node)
											+ heuristicDistance);
						}
					}
				}
			}
		}
		return result;
	}

	private List<Node> getNeighbors(Node node, Map<Location, Node> allNodes) {
		List<Node> neighbors = new ArrayList<Node>();
		Direction[] directions = allowDiagonal ? Direction.values() : Direction.MAIN_DIRECTIONS;
		for (Direction direction : directions) {
			if(node.canTravelFrom(direction)) {
				Location location = direction.fromLocation(node.getLocation());
				if (allNodes.containsKey(location)) {
					Node neighbor = allNodes.get(location);
					if(neighbor.canTravelTo(direction)) {
						neighbors.add(neighbor);
					}
				}
			}
		}
		return neighbors;
	}

	private List<Node> reconstructPath(Map<Node, Node> trail,
			Node goal) {
		List<Node> path = new ArrayList<Node>();
		if (trail.get(goal) != null) {
			path = reconstructPath(trail, trail.get(goal));
			path.add(goal);
		}
		return path;
	}
}
